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ABSTRACT 


Quadrotor  drones  pose  a  safety  hazard  when  operated  in  or  near  controlled 
airspace.  A  hazardous  quadrotor  could  be  intercepted  and  removed  by  another  quadrotor. 
In  this  thesis,  we  seek  to  determine  if  optimal  control  methods  outperform  missile  control 
methods  when  applied  to  a  quadrotor  drone  performing  an  intercept  with  a  moving  target. 
This  is  achieved  by  simulating  the  intercept  of  a  target  with  a  quadrotor  and  comparing 
the  performance  of  several  on-line  trajectory  planners.  Two  missile  control-based 
trajectory  planners,  pursuit  guidance  and  proportional  navigation,  are  compared  against 
an  optimal  control  trajectory  planner.  The  time  and  energy  used  by  a  simulated  quadrotor 
to  intercept  a  target  are  the  performance  measures  used  for  comparison.  The  trajectory 
planners  use  a  three-degree  of  freedom  model,  and  the  simulated  quadrotor  uses  a  six- 
degree  of  freedom  model.  Each  trajectory  planner  is  compared  in  a  crossing,  head-on,  and 
tail-chase  geometry.  All  of  the  on-line  results  are  compared  to  an  off-line  optimal 
solution.  The  results  show  that  the  off-line  optimal  control  method  performs  better  than 
the  on-line  trajectory  planners,  regardless  of  intercept  geometry  type.  The  proportional 
navigation  planner  has  the  best  performance  of  the  on-line  trajectory  planners. 
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I. 


INTRODUCTION 


Unmanned  aerial  systems  are  increasingly  common  in  both  military  and  civilian 
environments.  With  the  advent  of  inexpensive  direct  current  (DC)  motors  and  motor 
controllers,  quadrotor  unmanned  aerial  vehicles  (UAVs)  have  become  widely  available  in 
the  civilian  hobby  sector.  Systems  as  simple  as  line-of-sight  pilot-controlled  model 
aircraft  and  as  complex  as  autonomous  Global  Positioning  System  (GPS)  guided  camera 
systems  now  crowd  the  already  saturated  airspace.  As  these  unmanned  craft  have  become 
more  available,  regulating  their  use  has  become  more  challenging. 

The  Federal  Aviation  Administration  (FAA),  the  agency  responsible  for  the  safe 
use  of  airspace  nationwide,  has  taken  several  steps  to  educate  the  public  on  the  proper  use 
of  unmanned  aircraft.  This  approach  to  safety  has  its  limits  and  best  serves  amateur 
operators  who  have  sought  out  information  on  regulations  and  safety  restrictions.  Unlike 
a  licensed  pilot  who  is  required  to  submit  to  regular  oversight  by  the  FAA  when 
operating  manned  aircraft,  no  system  ensures  that  a  private  citizen  who  builds  or 
purchases  a  remotely  operated  drone  abides  by  the  applicable  airspace  regulations.  A  gap 
in  enforcement  capabilities  remains  in  locations  where  safety  of  flight  is  critical — namely 
in  and  around  controlled  airspace  at  airports. 

Several  commercially  available  systems  can  assist  in  the  enforcement  of 
regulations  for  remotely  operated  hobby  quadrotor  UAVs.  Most  commonly,  systems  like 
Skytracker  [1]  use  a  series  of  sensors  to  triangulate  the  rogue  UAV  based  on  signals 
emitted  by  the  quadrotor  and  its  ground  controller.  The  triangulated  ground  position  is 
then  forwarded  to  authorities  who  are  left  with  the  burden  of  getting  the  pilot  to  land  the 
system  safely.  In  some  cases,  the  detection  systems  may  also  be  equipped  with  a  jammer 
that  can  disable  a  system  and  cause  it  to  enter  a  fail-safe  hover  mode. 

More  aggressive  systems  are  being  developed  to  remove  rogue  UAVs  from 
controlled  airspace.  Some  systems  use  lasers  or  small  projectiles  to  immediately  remove 
the  safety  threat.  Although  firing  a  projectile  may  effectively  remove  the  UAV,  it  can 
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create  a  hazard,  especially  if  the  projectile  might  land  in  a  populated  area.  A  different 
method  is  needed  to  remove  a  rogue  UAV  in  an  area  where  shooting  it  down  is  unsafe. 

Arguably,  the  safest  method  of  removing  a  rogue  UAV  is  to  catch  it.  This  also 
allows  the  operator  of  a  registered  but  rogue  UAV  to  be  identified  so  that  a  fine  or  other 
penalty  may  be  assessed.  Companies  such  as  Guard  From  Above  use  trained  raptors  that 
grab  the  rogue  UAV  and  return  with  it  in  hand.  Once  captured,  the  UAV  is  no  longer  a 
safety  threat  and  does  not  fall  onto  people  or  property  [2].  This  approach  has  its  own  risks 
and  advantages,  considering  the  safety  of  the  bird  or  the  possibility  that  the  bird  may  drop 
the  UAV. 

If  we  can  design  a  system  for  airports  that  detects  small  rogue  aircraft  by  using  a 
radar  or  camera,  then  we  can  use  a  quadrotor  UAV  to  remove  the  rogue  safety  threat.  A 
central  control  system  can  be  used  to  send  a  “micro  interceptor”  to  intercept  the  rogue 
UAV  and  neutralize  it  by  capture  or  destructive  force,  as  appropriate.  The  purpose  of  this 
research  is  to  compare  different  methods  of  on-line  trajectory  planning  for  a  quadrotor 
micro  interceptor  used  in  this  capacity  to  intercept  a  moving  target. 

A.  BACKGROUND 

The  intercept  problem  has  analogues  in  several  fields.  Missile  guidance  is  directly 
related  because  it  shares  the  same  goal:  to  intercept  a  remotely  detected  target  on  the 
command  of  a  human  operated  system  and  prevent  the  target  from  potentially  causing 
harm  to  a  protected  area  or  object. 

An  active  missile  is  guided  to  a  target  based  on  energy  emitted  by  the  missile  that 
reflects  off  the  target,  usually  a  radar.  A  semi-active  missile  is  guided  to  a  target  based  on 
energy  emitted  by  a  separate  station  that  reflects  off  the  target,  such  as  a  transmitter 
located  at  the  launcher.  A  passive  missile  is  guided  to  a  target  based  on  measuring  energy 
that  is  emitted  by  the  target,  such  as  heat  or  radar  signals.  Hybrids  of  these  methods  are 
often  used  for  guidance  during  phases  of  flight  that  may  not  allow  for  useful  transfer  of 
information.  The  type  of  guidance  a  missile  uses  is  determined  by  the  type  of  platform 
that  fires  the  missile  and  the  threat  the  missile  is  expected  to  encounter. 
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In  this  research,  the  hypothetical  system  is  assumed  to  have  perfect  knowledge  of 
the  target  and  the  micro  interceptor.  Because  payload  weight  limits  on  the  interceptor 
preclude  the  use  of  robust  onboard  sensors  for  tracking  the  target,  the  burden  of  trajectory 
planning  is  assumed  by  the  ground  control  station.  This  allows  the  control  of  the 
interceptor  to  be  limited  to  a  command  position.  The  sequence  in  time  of  command 
positions  composes  the  command  trajectory.  The  interceptor  compares  its  known  position 
to  the  command  position  and  internally  produce  the  required  flight  control  inputs  to  fly 
the  commanded  trajectory.  Unlike  most  missiles,  this  system  receives  a  trajectory  to  fly 
from  the  ground  controller. 

A  missile  system  operates  in  a  highly  time-constrained  environment  because  often 
the  threats  to  such  a  system  move  quickly  and  in  ways  that  are  intentionally  difficult  to 
detect.  This  requires  that  the  missile  also  be  moving  quickly,  often  pushing  the  limits  of 
forces  applied  to  the  airframe.  Similarly,  this  research  examines  the  limits  of  the  micro 
interceptor  with  respect  to  power  consumption  and  time  performance,  assuming  a  threat 
with  comparable  flight  characteristics. 

B.  THESIS  OUTLINE  AND  OBJECTIVES 

In  this  research,  we  seek  to  determine  if  optimal  control  can  outperform  classic 
missile  control  methods  when  applied  to  quadrotor  drones  by  simulating  the  intercept  of  a 
target  with  a  quadrotor  drone  and  comparing  the  performance  measure  of  three  trajectory 
planners.  The  trajectory  planner  with  the  best  performance  in  simulation  represents  the 
best  planner  for  actual  flight  trajectory  planning. 

Applicable  background  in  guidance,  navigation,  and  control  (GNC)  and  a  brief 
outline  of  classic  missile  guidance  are  given  in  Chapter  II.  The  theoretical  background  of 
optimal  control  and  how  related  works  have  solved  similar  intercept  trajectory  planning 
problems  is  also  discussed. 

Next,  the  dynamic  model  for  a  simulated  quadrotor  is  presented  in  Chapter  III. 
The  assumptions  made  in  simplifying  the  dynamic  model  that  allow  simulation  using 
Simulink  and  MATLAB  are  stated  and  explained. 
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The  trajectory  planners  used  by  the  simulation  are  explained  in  Chapter  IV.  The 
point  mass  model  and  the  guidance  law  used  for  each  planner  to  create  a  trajectory  are 
discussed. 

The  Simulink  and  MATLAB  simulation  environment  is  explained  in  Chapter  V. 
The  function  of  every  piece  of  the  time-based  simulation  is  outlined  and  technical 
parameters  are  defined. 

The  framework  of  the  simulated  experiment  is  described  in  Chapter  VI.  Three 
intercept  geometries  are  defined,  and  the  method  of  evaluating  a  trajectory  planner  is 
established.  The  results  of  the  simulation  can  be  used  to  make  a  conclusion  based  on  the 
evaluation  of  the  trajectory  planner. 

The  results  of  the  simulation  are  presented  in  Chapter  VII.  Results  are  shown  as  a 
collection  of  trajectory  plots  and  flight  statistics,  grouped  by  intercept  geometry.  The 
flight  statistics  provide  information  about  the  experiment  that  are  not  observable  in  a 
trajectory  plot  (velocity  and  distance  to  target)  and  highlight  the  strengths  and 
weaknesses  of  each  planner  against  the  different  geometries. 

Finally,  a  conclusion  based  on  the  simulated  data  states  which  trajectory  planner 
has  the  highest  performance  in  Chapter  VIII.  A  summary  of  shortcomings  of  the  research 
as  well  as  areas  for  future  work  are  presented. 
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II.  LITERATURE  REVIEW  AND  RELATED  WORKS 


GNC  refers  to  the  process  of  solving  the  intercept  problem  according  to  Lin  in 
[3].  Lin  explains  that  typically,  this  problem  involves  an  intercept  vehicle  and  a  target, 
although  it  may  also  refer  to  other  problems  in  aerospace  or  robotics.  Lin  states  that 
navigation  refers  to  the  system  function  on  the  intercept  vehicle  that  determines  its 
attitude  and  position,  guidance  refers  to  the  system  function  that  determines  a  trajectory 
(physical  flight  path)  to  produce  an  intercept  with  the  target,  and  control  refers  to  the 
function  of  translating  guidance  commands  to  flight  surface  and  thrust  inputs.  Control 
ensures  that  the  vehicle  is  stable  in  flight  while  following  guidance  commands  and 
rejecting  system  disturbance  [3]. 

An  example  of  a  GNC  system  is  a  missile  system,  such  as  those  described  in  [4] 
and  [5].  In  these  systems,  a  missile  is  the  intercept  vehicle,  and  the  target  is  another 
missile  or  an  aircraft.  The  missile  may  use  an  inertial  navigation  system  or  an  external 
tracking  system  (such  as  a  radar)  to  determine  its  position.  This  raw  measurement  of 
position  and  attitude  is  filtered  with  a  Kalman  filter  (or  similar)  to  produce  a  quality 
estimate  of  the  actual  position  and  attitude  of  the  missile.  The  navigation  information  is 
compared  to  the  position  information  of  the  target  by  the  guidance  computer.  The 
guidance  computer  produces  a  command  trajectory  that  produces  an  acceptable  intercept. 
The  intercept  is  determined  based  on  an  acceptable  intercept  distance,  which  is 
determined  by  the  range  required  to  deploy  the  payload  (explosive  charge  in  the  case  of 
missiles). 

Missile  systems  solve  the  air-to-air  intercept  problem  effectively  and  serve  as  the 
foundation  for  solving  the  same  problem  applied  to  quadrotor  UAVs.  Methods  for 
quadrotor  intercept  have  been  explored  in  literature.  Pursuit  guidance,  a  missile  guidance 
law,  is  adapted  by  [6]  for  the  purposes  of  quadrotor  path  following  in  confined  spaces. 
Further,  proportional  navigation  (PN)  is  adapted  for  ground  target  intercepts  by  [7].  The 
ground  based  target  is  limited  to  two  dimensions  for  maneuvering,  and  the  quadrotor 
navigates  in  two  dimensions  while  maintaining  a  fixed  altitude.  A  similar  application  of 
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PN  is  explored  in  [8],  which  tracks  a  flying  target  although  in  this  case  the  target  is  free 
to  maneuver  in  all  three  dimensions,  the  problem  remains  constrained  in  altitude. 

In  addition  to  adapting  missile  guidance  law  to  generate  intercept  trajectories, 
researchers  have  applied  the  principles  of  optimal  control  to  intercept  trajectory  planning. 
A  time-optimal  trajectory  planner  is  developed  and  applied  to  a  quadrotor  in  [9]  and  [10], 
where  the  goal  is  to  move  to  a  series  of  fixed  waypoints.  Similarly,  optimal  trajectories 
for  moving  target  intercept  in  real  time  are  developed  by  [11]  and  references  therein.  A 
method  for  testing  trajectory  planners  is  explored  in  [12],  following  different  trajectory 
geometries  that  may  be  suited  for  specific  types  of  missions.  Further  integrated  mission 
planning  using  time  optimal  guidance  and  optimal  control  are  developed  by  [13]  and 
executed  in  hardware. 

Flight  control  of  a  quadrotor  is  also  widely  studied.  A  common  method  for 
producing  flight  commands  to  follow  a  trajectory  is  a  proportional-integral-derivative 
(PID)  controller.  A  PID  controller  seeks  to  minimize  the  error  between  a  reference  input 
and  measured  state.  PID  controllers  and  analysis  methods  outlined  by  [14]  can  be  applied 
to  the  flight  control  of  a  quadrotor.  These  methods  are  demonstrated  by  [15]  to  control 
the  thrust  produced  by  the  motors  in  order  to  execute  guidance  law.  Additionally,  [16] 
shows  a  similar  approach  but  instead  uses  an  optimal  linear  quadratic  regulator  (LQR) 
method  for  flight  control. 

Optimal  control  methods  differ  from  classic  control  methods  because  they  seek  to 
minimize  a  cost  function.  The  cost  function  and  control  input  are  related  by  a  function 
called  a  Hamiltonian,  which  is  a  function  of  system  states  and  co-states  [17]. 

Literature  widely  documents  the  challenges  associated  with  creating  an  on-line 
intercept  (an  intercept  in  real  time).  The  challenge  stems  from  knowing  only  where  the 
target  was  and  is  without  any  knowledge  of  where  the  target  is  going  or  how  it  will  arrive 
there.  The  solutions  to  the  on-line  intercept  problem  are  sub-optimal,  meaning  that  the 
cost  to  perform  an  intercept  will  not  be  the  theoretical  minimum. 

The  pursuit  guidance  and  proportional  navigation  methods  for  missile  guidance 
given  by  [4]  and  [5]  are  on-line  solutions  to  the  intercept  problem.  As  shown  in  [7],  those 
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guidance  methods  are  at  least  suitable  for  a  quadrotor  aircraft  performing  an  intercept 
with  a  ground  target.  Other  attempts,  such  as  in  [6],  demonstrate  that  proportional 
navigation  is  a  feasible  method  for  a  three-dimensional  (3D)  intercept  of  an  airborne 
target. 
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III.  DYNAMIC  MODELS 


A  model  is  used  to  represent  the  command  trajectories  as  well  as  the  simulated 
quadrotor.  A  point  mass  model  with  only  turn  rate  as  an  input  represents  the  command 
trajectories  created  by  pursuit  guidance  and  proportional  navigation  planners.  A  point 
mass  model  with  a  three-dimensional  acceleration  input  is  used  to  represent  the  command 
trajectory  for  the  model  predictive  control  (MPC)  planner.  The  point  mass  model  used  to 
represent  the  simulated  quadrotor  is  controlled  by  a  thrust  (force)  and  torque  input.  In  this 
section,  we  show  the  equations  of  motion  and  define  all  reference  frames  for  each  one  of 
the  point  mass  models. 

A.  COORDINATE  AND  AXIS  DEFINITION 

The  coordinate  system  used  for  the  inertial  frame  is  north  east  down  (NED),  as 
shown  in  Figure  1. 


Figure  1.  Inertial  Coordinates 

The  coordinates  of  the  target  and  the  interceptor  are  expressed  as  a  vector 
originating  from  the  inertial  frame 


P, 


P  = 


Py 


L  Pz\ 


(i) 
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The  trajectory  for  both  target  and  interceptor  is  expressed  as  a  discrete  position 
with  coordinates  p  .  Trajectories  are  generated  using  a  point  mass  model  represented  by 
three  degrees  of  freedom. 

B.  MODELING  ASSUMPTIONS 

Aerodynamics  are  not  modeled  for  either  the  quadrotor  model  or  the  trajectory 
model.  The  state  of  the  quadrotor  battery  is  assumed  to  remain  constant  for  duration  of 
flight,  providing  a  constant  voltage  and  current.  This  allows  the  motor  performance  to  be 
modeled  as  a  constant,  neglecting  any  change  in  thrust-to-throttle  caused  by  fluctuation  in 
battery  voltage.  Finally,  the  position  and  attitude  of  the  quadrotor  and  the  target  are 
assumed  to  be  known  at  all  times. 

C.  EQUATIONS  OF  MOTION 

Three-degree  of  freedom  (3 -DOF)  and  six-degree  of  freedom  (6-DOF)  models  are 
defined  in  this  section.  The  models  are  used  to  generate  trajectories  and  simulate  the 
flight  of  a  quadrotor.  They  are  implemented  using  MATLAB  S-functions. 

1.  Trajectory  3-DOF  Model 

Two  models  are  used  to  create  the  command  trajectory.  The  first  model  is  for  the 
pursuit  guidance  and  proportional  navigation  planners,  adapted  from  [18].  It  consists  of 
six  states 

0  0  0  1  0  OTp/ 

0  0  0  0  1  0  py 

0  0  0  0  0  1  p . 

(2) 

0  0  0  0  -^0  px 

0  0  0  ft)  0  0  py 

0  0  0  0  0  0  p_ 

with 
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(3) 


The  vector  p  is  the  position  of  the  trajectory  defined  in  Figure  1.  The  velocity  is 
the  vector  v ,  and  co  is  the  commanded  turn  rate  (control  input).  This  nonlinear  model 
uses  a  fixed  velocity,  and  the  control  input  is  described  in  a  later  section. 

The  model  predictive  controller  does  not  produce  a  commanded  turn  rate.  Instead, 
it  controls  a  three-dimensional  force  vector.  This  allows  a  linear  model  of  the  standard 
form  x  =  Ax  +  Bu  to  be  used  such  that 
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(4) 


The  control  input  in  (4)  is  the  force.  In  this  case,  force  is  defined  by  /  =  mu  with 
mass  m  —  1 .  This  allows  the  input  to  be  simplified  to 


u  =  a  = 


L«: 


(5) 


The  guidance  law  governing  the  control  input  for  this  planner  is  described  in  a 
later  section. 


2.  Aircraft  6-DOF  Model 

Equations  of  motion  representing  the  simulated  quadrotor  are  expressed  as  a  6- 
DOF  point  mass.  This  model  is  adapted  from  [19]  and  is  derived  using  the  Newton-Euler 
approach.  The  state  vector  for  the  simulated  quadrotor  is  given  as 
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co=  cox  .  (10) 

3x1 

In  Equations  (6)  through  (10),  the  vector  p  is  the  position  in  the  inertial  frame, 
the  vector  v  is  the  velocity  in  the  inertial  frame,  the  vector  ®  is  the  orientation  expressed 
as  Euler  angles,  and  co  is  the  angular  rate.  The  Euler  angles  are  defined  in  ORoll  is 
represented  by  the  angle  <f> ,  pitch  is  represented  by  the  angle  0 ,  and  the  angle  ;//  is  yaw. 
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Figure  2.  Roll,  Pitch,  and  Yaw  Angles 


The  time  derivatives  of  Equation  (6)  are  given  by 


P  =  v. 


roi 

0 

V  = 

0 

~Rih 

0 

_<?J 

1 

\£ 

_ 1 

<i>  =  G“V 


(ii) 


(12) 


(13) 


&>  =  /  1  [r-skew{co)Jco) .  (14) 

The  force  due  to  gravity  is  g  ,  ft  is  the  force  due  to  thrust,  m  is  the  mass  of  the 
quadrotor,  and  r  is  the  total  torque  produced  by  the  four  motors.  The  system  is  a 
function  of  the  control  inputs  ft  and  r  illustrated  in  Figure  3. 
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Figure  3.  Control  Inputs 


Additionally,  the  components  of  (13)  and  (14)  are  defined  by 


and 


Q  = 


0  -sin# 
cos  (j)  sin  (j)  cos# 
-sin  (j)  cos  (j)  cos# 
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(16) 


(17) 


The  components  Jx,  J  y ,  and  J  are  the  inertial  moments  along  each  axis.  The  rotation 
matrices  are  defined  as  for  each  axis  as 


R. 


cos  y/  -sin  y/  0 
sin  y/  cos  y/  0 

0  0  1 


Re  = 


cos  #  0  sin  # 

0  1  0 
-sin#  0  cos# 


(18) 


(19) 


14 


and 
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0 
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0 

COS  <j) 

sin^ 


0 

-sin^ 

cos^ 


Equations  (18)  to  (20)  are  combined,  producing  a  single  rotation  matrix 


(20) 


Rlh=RvReRr  (21) 

Orientation  of  the  quadrotor  is  expressed  in  the  inertial  frame  as  an  orientation  defined  by 
Euler  angles  using  Rib . 
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IV.  INTERCEPT  TRAJECTORY  PLANNER  TYPES 


There  are  three  trajectory  planners  used  in  this  research.  Pursuit  guidance  (PG) 
and  PN  are  commonly  used  for  missile  trajectories.  MPC  is  widely  applied  in  the  field  of 
controls  and  is  adapted  to  trajectory  planning.  Each  of  these  methods  uses  the  current 
position  of  the  interceptor  and  the  target  in  order  to  calculate  an  intercept  trajectory. 

The  linear  quadratic  tracker  (LQT)  is  an  optimal  control  method  used  to  track  a 
reference  trajectory.  This  trajectory  planner  is  offline  (not  real  time)  and  serves  as  a 
benchmark  to  compare  the  performance  of  the  three  on-line  trajectory  planners.  In  this 
section,  we  describe  all  four  of  these  trajectory  planning  methods. 

A.  PURSUIT  GUIDANCE 

PG  is  the  simplest  of  the  trajectory  planners.  The  method  is  adapted  from  missile 
guidance  law  that  produces  an  intercept  with  a  target  by  pointing  the  heading  of  the 
missile  at  the  target  for  the  duration  of  flight.  To  achieve  this,  the  only  input  the 
algorithm  needs  is  the  angle  between  the  missile  and  the  target.  This  is  used  to  generate  a 
turn  rate  0)  that  controls  the  heading  of  the  missile. 

To  adapt  this  method  for  the  purpose  of  an  intercept  between  two  quadrotors,  a 
point  mass  model  is  used  to  represent  the  interceptor  to  produce  an  on-line  command 
trajectory.  The  point  mass  model  is  manipulated  with  a  single  input  that  controls  the  turn 
rate,  constrained  to  two  dimensions  in  the  x-y  plane  (a  yawing  turn  along  the  z-axis). 

This  control  excludes  the  z-altitude  axis,  which  is  assumed  to  be  a  relatively 
steady  parameter  in  a  quadrotor  target.  The  amount  of  motion  in  this  axis  is  likely  to  be 
significantly  lower  than  in  the  x-y  plane  because  many  commercially  available 
quadrotors  are  controlled  with  the  altitude  fixed  to  minimize  the  complexity  of  flight 
control  for  the  pilot. 

From  [18],  the  turn  rate  is  determined  by 
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The  gain  k  can  be  tuned  to  produce  more  or  less  aggressive  turning  maneuvers. 
In  the  case  of  a  missile  in  flight,  this  may  be  an  important  consideration  given  that  the 
forces  acting  on  the  missile  may  exceed  the  design  rating.  In  the  case  of  a  quadrotor, 
however,  the  force  design  margins  are  not  as  tight.  The  line  of  sight  vector  P  is  defined 
by 

P=  X,~Xi  (23) 

y,-yt. 

which  is  the  vector  formed  by  the  position  difference  of  the  interceptor  and  the  target. 
Vectors  used  in  the  pursuit  guidance  planner  and  Equation  (23)  are  illustrated  in  Figure  4. 

P,  ={x„yt) 


vi 


Figure  4.  Vector  Definitions  for  Pursuit  Guidance 

This  trajectory  planner  assumes  a  fixed  velocity.  The  starting  position  and 
velocity  are  given  as  initial  conditions  in  the  S-function  used  to  implement  the 
differential  equation,  and  velocity  is  selected  to  be  two  meters  per  second  faster  than  that 
of  the  target.  This  ensures  that  the  interceptor  will  catch  the  target  in  a  worst-case 
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scenario  tail-chase  geometry.  For  physical  implementation,  the  velocity  should  be 
selected  to  be  slightly  below  the  known  maximum  flight  speed  of  the  interceptor. 

B.  PROPORTIONAL  NAVIGATION 

PN  is  an  improvement  on  the  PG  planner.  Instead  of  pointing  the  heading  of  the 
trajectory  directly  at  the  target  (as  with  PG),  PN  steers  the  trajectory  on  a  course  that 
causes  an  intercept  to  occur  in  the  path  of  a  non-maneuvering  target.  The  resulting 
trajectory  intercepts  a  target  by  maintaining  a  constant  line-of-sight  angle.  To  achieve 
this,  the  additional  input  of  closing  velocity  must  be  provided  to  calculate  turn  rate. 

To  adapt  this  method  for  the  purpose  of  an  intercept  between  two  quadrotors,  a 
point  mass  model  is  used  to  represent  the  interceptor  to  produce  an  on-line  command 
trajectory.  The  point  mass  model  is  manipulated  with  a  single  input  that  controls  the  turn 
rate,  constrained  to  two  dimensions  in  the  x-y  plane  as  previously  described  with  the 
pursuit  guidance  planner.  The  same  3-DOF  model  is  used  with  the  input  co  given  in  [18] 
as 


co  = 


kv  6, 


LOS 


vi  cost  0i  -  6los  ) 


(24) 


where  k  is  a  gain,  vc  is  the  closing  velocity  between  interceptor  and  target,  vt  is  the 
velocity  of  the  interceptor,  and  0LOS  is  the  line-of-sight  angle  between  vc  and  the  y-axis. 


The  vectors  used  for  PN  are  illustrated  in  Figure  5.  The  relative  velocity  vr 

between  target  and  interceptor  is  defined  for  reference.  As  with  the  PG  planner,  the  PN 
planner  is  initialized  with  a  starting  position  and  velocity  that  remains  constant 
throughout  the  flight. 
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V, 


Figure  5.  Vector  Definitions  for  Proportional  Navigation 


C.  OPTIMAL  TRAJECTORIES 

The  off-line  optimal  trajectory  planner  is  adapted  from  [17].  The  LQR  method 
solves  the  optimal  trajectory  problem  given  a  system  of  the  form 

x  =  Ax  +  Bu  (25) 

where  x  is  the  state  vector,  x  is  the  time  derivative  of  the  state  vector,  and  u  is  the  input 
vector  with  A  and  B  as  constant  matrices.  The  system  is  subject  to  the  cost 

1  1  'f 

J  =  —  xT  (tf  )Hx(tf )  +  —  J  [  (t)Qx(t)  +  uT  (t)Ru(t)]dt  (26) 

2  2  to 

with  H ,  Q  ,  and  R  as  real  and  positive  definite  matrices.  From  the  cost,  the  Hamiltonian 
is  formed 


H(x,u,  p,t )  =  —  | ~xT(t)Qx(t)  +  uT(t)Ru(t)^  +  pT  [Ax  +  Bu] 


(27) 


where  p  is  the  costate  vector.  The  necessary  conditions  for  optimality  are  given  by 


x 


- =  Ax  +  Bu  , 

dp 


(28) 
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dH  n 

P  =  -—  =  -Qx-A  p. 

ox 


(29) 


and 


0  =  —  =  Ru  +  BTp . 

du 


(30) 


Finally,  the  optimal  input  u  (t)  for  an  open  loop  system  is  given  by 


u  (t)  = -R  l BT p(t) . 


(31) 


A  more  useful  solution  is  the  LQT  method.  Instead  of  finding  an  optimal  control 
that  drives  the  state  to  zero,  the  LQT  finds  an  optimal  control  that  drives  the  state  to  a 
time  varying  reference  trajectory.  In  this  case,  the  cost  J  is  given  by 


J  =  - 
2 


tf 

\ [x(tf )  - r(tf )]  H [x(tf )  -  r(tf )] + J  \^x(t)  -  r(t)f  Q[x(t)  - r(t )]  +  u  (t)Ru{t)^lt  (32) 


where  r  is  the  reference  trajectory.  The  Hamiltonian  is  given  by 


1  r  T  -| 

H(x,u,p,t)  =  —  [v(t)-r(t)]  Q[x(t)~  r(t)]  +  uT (t)Ru(t)  +  pT  [A.r  +  Bu\ .  (33) 


The  necessary  conditions  of  optimality  become 


.  dH  A  D 

x  = =  Ax  +  Bu 

dp 


P  =  -—  =  -Q(x~r)-A  p, 

ox 


(34) 

(35) 


and 


0  =  — -  =  Ru  +  BT  p . 

du 


(36) 


Finally,  the  optimal  control  becomes 


u  (t)  =  -R  B  p(t). 
21 


(37) 


This  method  is  used  to  generate  an  optimal  trajectory  to  serve  as  a  comparison  to 
the  on-line  solutions  created  in  the  simulation. 

D.  MODEL  PREDICTIVE  CONTROL 

MPC  is  described  in  [20]  and  [21].  MPC,  like  optimal  control,  produces  a  control 
output  for  a  system  that  minimizes  a  cost  function.  Unlike  LQT,  MPC  uses  a  model  of  the 
system  to  predict  the  output  and  internal  state  at  given  time  in  the  future.  The  forward 
looking  time  period  is  called  the  horizon.  MPC  calculates  an  optimized  control  input  at 
every  time  step  considering  the  predicted  output  at  the  time  horizon.  This  behavior  is 
desirable  for  a  wide  range  of  applications  but  may  be  particularly  well  suited  for  intercept 
trajectory  planning. 

In  addition  to  predicting  the  output  and  state  of  a  controlled  system,  MPC  may 
also  be  used  to  constrain  internal  states  of  the  system.  This  is  applied  to  the  intercept 
problem  to  limit  the  velocity  of  the  command  trajectory.  Limiting  the  force  (system 
input)  applied  to  the  trajectory  planner  will  not  prevent  the  velocity  of  the  command 
trajectory  from  exceeding  the  capabilities  of  the  quadrotor.  Instead,  a  constraint  is  created 
on  the  velocity  state  in  the  MPC’s  internal  model  of  the  system. 

The  controller  used  in  this  research  is  included  in  the  MATLAB  and  Simulink 
controls  library  as  the  MPC  toolbox.  The  toolbox  documentation  [22]  describes  the  cost 
function  optimized  by  the  MPC  controller.  The  cost  function  is  similar  to  the  function 
given  in  Equation  (32)  but  includes  additional  terms  that  account  for  algebraic 
constraints.  Before  the  block  can  be  used  in  Simulink,  an  MPC  object  must  be  present  in 
the  workspace.  This  is  achieved  before  loading  the  simulation  using  a  MATLAB  script. 
The  MPC  object  is  initialized  with  the  system  model  in  the  form  x  =  Ax  +  Bu  and 
y  -  Cx  +  Du  ,  specifically, 
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(39) 


System  state  constraints  are  specified  to  limit  the  velocity  in  the  form  of 
Eu  +  Fy  <G  .  This  is  given  by 
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where  vmax  is  the  maximum  velocity  in  each  direction.  For  this  simulation,  vmax  is  chosen 
to  be  a  soft  constraint  of  nine  meters  per  second.  The  initialization  function  also  sets  the 
time  step  T  =0.1  s,  prediction  horizon  p  —  50 ,  and  the  control  horizon  m  -  20  . 

The  MPC  trajectory  planner  is  shown  in  Figure  6.  The  Simulink  MPC  block 
inputs  are  the  reference  (ref)  signal  and  measured  outputs  (mo).  The  MPC  block  outputs 
are  manipulated  variables  (mv).  For  the  MPC  trajectory  planner,  the  ref  signal  is  the 
target  trajectory,  the  mo  signal  is  the  current  position  of  the  trajectory,  and  the  mv  signal 
is  the  force  input  to  the  S-function  for  trajectory  dynamics. 
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Figure  6.  MPC  Trajectory  Planner  Description 
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V.  SIMULINK  IMPLEMENTATION 


To  examine  the  performance  of  the  trajectory  planners,  a  simulated  experimental 
environment  was  created  using  MATLAB  and  Simulink.  The  environment  uses  the 
equations  of  motion  previously  described  to  simulate  the  flight  of  the  target,  the  intercept 
trajectory,  and  the  flight  of  the  quadrotor.  This  is  achieved  using  a  combination  of  user 
defined  MATLAB  functions  and  S-functions.  The  Simulink  models  are  described  in  this 
section,  and  the  supporting  MATLAB  scripts  and  functions  are  provided  in  Appendix  B. 

A.  OVERVIEW 

Each  simulated  flight  starts  the  target,  command  trajectory,  and  quadrotor  at  a 
specified  starting  point  with  an  initial  velocity.  These  parameters  are  defined  as  a  part  of 
geometry  selection.  Additionally,  each  simulated  flight  uses  one  trajectory  planner.  The 
trajectory  planner  and  geometry  are  selected  before  the  simulation  begins. 

A  block  diagram  showing  the  relationship  of  each  component  of  the  simulation  is 
shown  in  Figure  7.  The  target  dynamics,  trajectory  planner,  and  quadrotor  dynamics  are 
differential  equations  implemented  with  an  S-function.  The  flight  controller  is  implemented 
as  a  MATLAB  function.  The  3D  visualizer  uses  blocks  from  the  virtual  reality  toolbox  to 
create  a  virtual  environment  to  observe  the  position  and  attitude  of  the  quadrotor 
throughout  the  intercept.  Trajectory  plots  and  flight  data  are  the  experimental  results. 


Figure  7.  Structure  of  Simulation 
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B.  TRAJECTORY  PLANNERS 

The  trajectory  planners  are  illustrated  in  Figure  8.  Target  position  and  velocity  are 
inputs  and  the  command  trajectory  is  the  output.  The  guidance  law  block  is  implemented 
using  a  MATLAB  function,  and  the  interceptor  dynamics  block  is  implemented  with  an 
S-function. 


Figure  8.  Trajectory  Planner  Block  Diagram 


C.  QUADROTOR  FLIGHT  CONTROL 

The  purpose  of  the  flight  controller  is  to  produce  thrust  and  torque  commands  to 
fly  the  quadrotor  to  the  command  trajectory.  Because  the  effects  of  aerodynamic  drag  are 
not  modeled,  the  flight  controller  must  impose  a  maximum  velocity  that  is  realistic  of  a 
quadrotor  experiencing  those  effects.  To  do  this,  two  PID  controllers  are  used  to  isolate 
velocity  as  an  intermediate  variable  which  can  be  manipulated. 

The  configuration  of  the  PID  controllers,  adapted  from  [15],  is  shown  in  Figure  9. 
Each  controller  is  implemented  with  a  Simulink  PID  block.  Position,  velocity,  gravity, 
and  force  are  all  represented  by  three-dimensional  vectors. 
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Figure  9.  PID  Configuration  in  Flight  Controller 


The  configuration  of  each  PID  block  is  shown  in  Table  1.  Saturation  is  used  to 
control  the  maximum  velocity  that  the  quadrotor  flies  as  it  moves  to  the  command 
position. 


Table  1.  Simulink  PID  Block  Parameters 


Block  Parameter 

Position-to- velocity 

Velocity-to-force 

Proportional  (P) 

1.3 

1.5 

Integral  (I) 

0.1 

0.01 

Derivative  (D) 

0.7 

0.7 

Filter  Coefficient  (N) 

100 

100 

Saturation  Limit 

±20 

±10 

The  command  thrust  ft  and  command  torque  z  are  produced  based  on  input 

force,  quadrotor  orientation  ® ,  and  angular  rate  co  as  shown  in  Figure  10.  The  flight 
commands  block  is  a  MATLAB  function  that  creates  command  thrust  and  a  command 
orientation  ®OM, .  The  orientation  error  is  produced  by  subtracting  quadrotor  orientation 

from  command  orientation.  A  PID  block  is  used  to  produce  command  torque  based  on 
orientation  error. 
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/, 


Figure  10.  Flight  Controller  Block  Diagram 


Adapted  from  [19],  the  command  orientation  is  the  vector 


with  components 


You, 


(41) 


9  ,=  tan 

out 


-1 


Fx  cos(^)  +  F  sin(^) 


(42) 


tu,  = tan  ' 


Fx  sin(^)  -  Fv  cos(^)  cos(6>0„, ) 


(43) 


and  y/out  set  to  a  constant  zero.  This  eliminates  quadrotor  yaw  through  the  course  of 
flight.  Command  thrust  is  given  by 


ft= - ^ - .  (44) 

cos(Oout)cos(0out) 

D.  3D  VISUALIZER 

The  3D  visualizer  animates  the  intercept  in  a  virtual  environment.  The  visualizer 
uses  a  computer  aided  design  (CAD)  model  and  Simulink  VR  toolbox.  The  target 
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position,  quadrotor  position,  and  quadrotor  orientation  are  inputs  to  the  Simulink  VR  sink 
as  shown  in  Figure  1 1 . 


Figure  11.  3D  Visualizer  Block  Diagram 


The  quadrotor  and  target  represented  in  a  virtual  world  are  shown  in  Figure  12. 
The  virtual  world  is  a  200  m  square,  so  the  size  of  the  quadrotor  and  target  are 
exaggerated.  The  target  is  represented  by  a  red  sphere,  and  the  quadrotor  is  represented 
by  a  CAD  model. 


Figure  12.  3D  Visualizer  Virtual  World 
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The  quadrotor  CAD  model  used  by  the  visualizer  is  shown  in  Figure  13.  The 
model  is  based  on  a  typical  quadrotor  configuration. 


Figure  13.  CAD  Model  of  Quadrotor 
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VI.  SIMULATION  DESIGN 


A.  OVERVIEW 

The  purpose  of  the  simulation  is  to  compare  the  performance  of  each  of  the 
trajectory  planners.  The  simulation  produces  an  intercept  trajectory  which  is  flown  by  a 
simulated  quadrotor.  The  energy  used  by  the  simulated  quadrotor  and  intercept  time  are 
recorded.  Motor  power  consumption  rates  for  a  hypothetical  quadrotor,  measured  and 
listed  in  Appendix  A,  are  applied  to  the  simulation  to  estimate  the  total  power 
consumption  per  flight.  Based  on  [4]  and  [5],  some  planners  may  have  different 
performance  based  on  the  intercept  geometry;  therefore,  each  planner  is  evaluated  in 
three  types  of  intercept  geometries.  This  is  achieved  by  varying  target  trajectory  start 
location,  heading,  and  velocity. 

B.  SIMULATION  ASSUMPTIONS 

Some  assumptions  are  made  to  narrow  the  scope  of  the  simulation.  The 
simulation  assumes  that  the  trajectory  planner  has  perfect  knowledge  of  the  target  and 
interceptor  position.  It  also  assumes  that  the  target  has  constant  velocity  and  heading. 
Additionally,  an  intercept  is  considered  to  occur  when  the  distance  between  the  trajectory 
and  target  is  below  the  intercept  threshold  of  five  meters.  This  threshold  is  arbitrary  for 
this  experiment  but  should  be  adjusted  based  on  required  distance  to  deploy  payload  at 
intercept.  After  an  intercept  occurs,  the  simulation  is  complete,  and  target  tracking  after 
intercept  is  not  evaluated. 

C.  DEFINITION  OF  FLIGHT  PERFORMANCE  MEASURE 

Two  metrics  are  used  to  compare  the  performance  of  each  trajectory.  First,  the 
total  energy  spent  to  achieve  intercept  is  calculated.  This  is  the  energy  used  by  the 
simulated  quadrotor  as  it  flies  the  command  trajectory.  Second,  the  time  to  achieve 
intercept  is  recorded.  This  is  the  time  that  the  distance  between  the  target  and  command 
trajectory  is  less  than  the  intercept  threshold.  The  results  are  compared  and  evaluated  to 
determine  which  trajectory  planner  is  fastest  and  which  uses  the  least  energy. 
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D.  INTERCEPT  GEOMETRY  TYPES 


The  simulated  environment  is  a  three-dimensional  200-m  arena.  This  allows  the 
simulation  to  span  a  realistic  distance  for  an  air-to-air  intercept.  The  x  and  y  axes  are  of 
most  interest,  and  for  clarity  the  results  are  displayed  in  the  x-y  plane. 

Three  types  of  intercept  geometry  are  simulated  with  a  non-maneuvering  target. 
This  means  that  the  target  maintains  constant  velocity  without  change  in  heading. 
Investigating  the  performance  of  the  trajectory  planners  against  a  maneuvering  target  is 
left  for  future  work. 

Crossing,  head-on,  and  tail-chase  geometry  for  a  non-maneuvering  target  are 
shown  in  Figure  14.  The  target  is  represented  in  red,  and  the  interceptor  is  represented  in 
blue. 


Figure  14.  Geometry  Types 

The  starting  positions  and  headings  for  the  target  and  interceptor  for  each 
geometry  are  listed  in  Table  2.  All  headings  are  measured  from  North  (x-axis)  and 
positions  are  in  meters  with  the  format  (x,  y,  z)  ■  In  all  cases,  the  target  has  a  constant 
velocity  of  seven  meters  per  second,  and  the  interceptor  trajectory  starts  at  rest  at  the 
same  height  above  ground  as  the  target.  The  simulated  quadrotor  starts  at  the  ground 
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(z  =  0)  but  at  the  same  x-y  position  as  the  interceptor  trajectory,  thereby  including 
takeoff  in  the  simulated  flight. 


Table  2.  Target  and  Interceptor  Starting  Positions  and  Headings 


Crossing 

Head-on 

Tail-chase 

Target 

(50,  100,  2),  270° 

(100,  25,  2),  195° 

(-50,  -25,  2),  020° 

Interceptor 

(-100,  25,0) 

(-100,  25,0) 

(-100,  25,0) 

E.  SIMULATED  EXPERIMENTAL  SETUP 

To  compare  the  performance  of  the  trajectory  planners  in  simulation,  the 
experiment  was  implemented  as  a  MATLAB  function.  The  inputs  to  the  experiment 
function  were  interceptor  starting  position,  target  starting  position,  target  speed,  and 
target  heading.  When  the  experiment  function  is  called,  it  runs  the  Simulink  experimental 
environment  described  in  Figure  7.  When  the  Simulink  simulation  is  complete,  the 
experiment  function  saves  the  trajectory  plot  and  flight  data  produced  by  the  simulation. 
The  function  returns  the  intercept  time  and  energy  used.  A  MATLAB  script  calls  the 
experiment  function  for  each  trajectory  planner  and  geometry  type,  totaling  nine 
experiments.  The  script  then  plots  the  time  and  energy  used  for  each  experiment  for 
comparison. 

The  results  of  the  nine  experiments  are  then  compared  to  the  baseline  (LQT) 
intercept  results  described  in  the  Optimal  Trajectories  section.  The  LQT  results  are 
computed  using  a  single  MATLAB  script  that  solves  Equations  (32)  through  (37)  to 
compute  an  optimal  intercept  trajectory.  The  trajectory  is  saved  and  loaded  into  a  look-up 
table  in  the  experimental  environment  acting  in  place  of  the  trajectory  planner  block.  An 
experiment  is  run  for  each  geometry  type  using  the  LQT  trajectories,  which  allows  the 
on-line  trajectories  to  be  compared  to  the  optimal  LQT  trajectory. 
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VII.  SIMULATION  RESULTS 


Simulation  results  for  the  three  on-line  trajectory  planners  are  presented 
individually,  organized  by  intercept  geometry  first,  then  by  planner  type.  The  LQT 
results,  simulated  by  a  quadrotor  as  if  they  were  generated  in  real  time,  are  presented 
following  the  on-line  results.  All  results  are  then  compiled  into  a  single  figure  for 
comparison. 

The  results  for  each  experiment  include  a  trajectory  plot  and  simulated  quadrotor 
flight  statistics.  Each  trajectory  plot  shows  the  target  trajectory  (red),  the  command 
trajectory  (yellow),  and  the  trajectory  flown  by  the  simulated  quadrotor  (blue).  The  flight 
statistics  show  velocity  of  the  quadrotor  (blue),  interceptor  (red),  and  the  command 
trajectory  position  error.  The  closest  point  of  approach  (CPA)  is  marked  on  the  command 
trajectory  position  error  by  a  black  star. 

The  optimal  intercept  (LQT  solution)  for  each  geometry  is  shown  in  Figure  15. 
The  grid  spacing  represents  50  m,  and  the  start  positions  are  listed  in  Table  2.  For 
example,  the  crossing  geometry  shows  the  target  starting  at  the  position  (50,100,2)  and 

flying  west.  The  interceptor  trajectory  is  shown  starting  at  the  position  (-100,25,0)  and 

flying  north  towards  the  target,  gradually  turning  and  following  to  the  west.  These  off¬ 
line  results  are  presented  for  comparison  to  the  on-line  results. 


Head-on 


Tail-chase 


Figure  15.  Optimal  LQT  Trajectories  for  Each  Geometry 
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The  trajectory  plot  for  the  MPC  planner  with  crossing  geometry  is  shown  in 
Figure  16.  The  command  trajectory  does  not  immediately  turn  towards  the  target.  This 
turn  delay  corresponds  to  the  number  of  forward-looking  time  steps  scheduled  for  the 
MPC  planner.  Although  the  first  leg  of  the  intercept  is  problematic,  the  flight  path 
following  the  turn  is  acceptable.  The  quadrotor  tracks  the  command  trajectory  with  ease, 
and  the  geometry  eventually  resembles  a  tail-chase. 


Figure  16.  Trajectory  Plot  of  MPC  Planner  and  Crossing  Geometry 

The  flight  statistics  for  the  MPC  planner  in  a  crossing  geometry  are  shown  in 
Figure  17.  The  CPA  occurs  slightly  after  20  s,  then  the  position  error  remains  constant  at 
15  m  for  the  remainder  of  the  simulation.  Because  the  position  error  does  not  drop  below 
the  intercept  threshold  of  five  meters,  this  simulation  does  not  meet  the  intercept  criteria. 
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Figure  17.  Flight  Statistics  for  MPC  Planner  and  Crossing  Geometry 

The  trajectory  plot  for  the  PN  planner  with  crossing  geometry  is  shown  in  Figure 
18.  The  command  trajectory  produces  an  acceptable  intercept. 


Y(m) 

Figure  18.  Trajectory  Plot  of  PN  Planner  and  Crossing  Geometry 

The  flight  statistics  for  the  PN  planner  in  a  crossing  geometry  are  shown  in  Figure 
19.  The  CPA  occurs  at  the  time  of  intercept,  approximately  17  s. 
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Figure  19.  Flight  Statistics  of  PN  Planner  and  Crossing  Geometry 


The  trajectory  plot  for  the  PG  planner  with  crossing  geometry  is  shown  in  Figure 
20.  The  command  trajectory  produces  an  acceptable  intercept,  and  the  simulated 
quadrotor  exhibits  slight  tracking  error  through  the  turn. 


Figure  20.  Trajectory  Plot  of  PG  Planner  and  Crossing  Geometry 
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The  flight  statistics  for  the  PG  planner  in  a  crossing  geometry  are  shown  in  Figure 
21.  The  CPA  occurs  at  the  time  of  intercept,  approximately  31  s.  In  this  scenario,  the 
geometry  begins  as  a  crossing  situation.  As  the  interceptor  maneuvers,  the  geometry 
begins  to  look  like  a  tail-chase,  and  the  closing  rate  decreases.  This  geometry  shift  is 
observed  as  a  change  in  slope  in  the  position  error  plot,  occurring  at  approximately  13  s. 


15 


0  ^ - 1 - 1 - 1 - 1 - 1 - 1 - 

0  5  10  15  20  25  30  35 

Time  (s) 


Figure  21.  Flight  Statistics  of  PG  Planner  and  Crossing  Geometry 

The  trajectory  plot  for  the  MPC  planner  with  head-on  geometry  is  shown  in 
Figure  22.  The  command  trajectory  produces  an  acceptable  intercept  despite  a  slight 
perturbation  during  the  first  half  of  the  flight.  This  disturbance  is  also  observed  in  Figure 
16.  In  both  cases,  the  disturbance  is  overcome  after  the  controller  has  more  time 
observing  the  system.  Other  than  the  disturbance,  the  flight  path  is  generally  direct  and 
the  intercept  is  not  affected  by  steady- state  error. 
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Figure  22.  Trajectory  Plot  of  MPC  Planner  and  Head-on  Geometry 


The  flight  statistics  for  the  MPC  planner  in  a  head-on  geometry  are  shown  in 
Figure  23.  The  CPA  occurs  at  the  time  of  intercept,  at  approximately  13  s. 
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Figure  23.  Flight  Statistics  of  MPC  Planner  and  Head-on  Geometry 
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The  trajectory  plot  for  the  PN  planner  with  head-on  geometry  is  shown  in  Figure 
24.  The  command  trajectory  produces  an  acceptable  intercept,  and  the  simulated 
quadrotor  shows  minimal  tracking  error. 


Y(m) 

Figure  24.  Trajectory  Plot  of  PN  Planner  and  Head-on  Geometry 

The  flight  statistics  for  the  PN  planner  in  a  head-on  geometry  are  shown  in  Figure 
25.  The  CPA  occurs  at  the  time  of  intercept,  at  approximately  13  s.  The  slope  of  the 
position  error  is  constant,  indicating  that  very  little  maneuvering  occurs  throughout  the 
flight.  This  also  reflects  the  direct  nature  of  the  flight  path,  highlighting  a  potential 
strength  of  the  PN  method. 
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Figure  25.  Flight  Statistics  of  PN  Planner  and  Head-on  Geometry 

The  trajectory  plot  for  the  PG  planner  with  head-on  geometry  is  shown  in  Figure 
26.  The  command  trajectory  produces  an  acceptable  intercept,  and  the  simulated 
quadrotor  shows  minimal  tracking  error. 


Y(m) 

Figure  26.  Trajectory  Plot  of  PG  Planner  and  Head-on  Geometry 
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The  flight  statistics  for  the  PG  planner  in  a  head-on  geometry  are  shown  in  Figure 
27.  The  CPA  occurs  at  the  time  of  intercept,  at  approximately  13  s.  The  slope  of  the 
position  error  is  constant  until  the  last  second  of  intercept.  This  change  corresponds  to  a 
turn  in  the  flight  path.  Although  this  qualifies  as  an  intercept,  the  position  of  the 
trajectory  lies  to  the  side  of  the  target  instead  of  ahead.  This  may  be  less  ideal  depending 
on  the  payload. 
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Figure  27.  Trajectory  Plot  of  PG  Planner  and  Head-on  Geometry 

The  trajectory  plot  for  the  MPC  planner  with  tail-chase  geometry  is  shown  in 
Figure  28.  The  simulated  quadrotor  shows  some  tracking  error  through  the  turn.  After  the 
turn,  the  scenario  becomes  a  tail-chase.  As  with  the  crossing  geometry  simulation,  a 
steady-state  tracking  error  occurs,  and  the  command  trajectory  fails  to  reach  the  target. 
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Figure  28.  Trajectory  Plot  of  MPC  Planner  and  Tail-chase  Geometry 

The  flight  statistics  for  the  MPC  planner  in  a  tail-chase  geometry  are  shown  in 
Figure  29.  The  CPA  occurs  just  before  25  s,  and  the  position  error  becomes  a  constant  of 
approximately  17  m.  This  does  not  produce  an  acceptable  intercept  because  the  steady- 
state  error  is  above  the  intercept  threshold. 


Figure  29.  Flight  Statistics  of  MPC  Planner  and  Tail-chase  Geometry 
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The  trajectory  plot  for  the  PN  planner  with  tail-chase  geometry  is  shown  in  Figure 
30.  This  straight-line  flight  path  shows  no  maneuvering,  thereby  producing  a  direct 
intercept.  These  results  highlight  the  effectiveness  of  the  PN  trajectory  planner  because 
there  is  no  significant  turning  required  by  the  simulated  quadrotor.  This  produces 
minimal  tracking  error  and  minimal  energy  wasted  in  turning,  making  the  maneuver 
efficient. 
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Figure  30.  Trajectory  Plot  of  PN  Planner  and  Tail-chase  Geometry 

The  flight  statistics  for  the  PN  planner  in  a  tail-chase  geometry  are  shown  in 
Figure  31.  The  CPA  occurs  at  the  time  of  intercept,  approximately  19  s.  The  direct  flight 
path  is  reflected  by  a  constant  slope  in  the  position  error  plot.  These  results  could  only  be 
made  more  efficient  by  increasing  the  speed  of  the  command  trajectory. 


45 


Figure  31.  Flight  Statistics  of  PN  Planner  and  Tail-chase  Geometry 

The  trajectory  plot  for  the  PG  planner  with  tail-chase  geometry  is  shown  in  Figure 
32.  The  simulated  quadrotor  shows  some  tracking  error  through  the  turn. 


Y(m) 

Figure  32.  Trajectory  Plot  of  PG  Planner  and  Tail-chase  Geometry 

The  flight  statistics  for  the  PG  planner  in  a  tail-chase  geometry  are  shown  in 
Figure  33.  The  CPA  occurs  at  the  time  of  intercept,  approximately  22  s. 
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Figure  33.  Flight  Statistics  of  PG  Planner  and  Tail-chase  Geometry 

The  trajectory  plot  for  the  LQT  solution  for  crossing  geometry  is  shown  in  Figure 
34.  The  simulated  quadrotor  shows  some  tracking  error  through  the  turn. 
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Figure  34.  Trajectory  Plot  of  LQT  Solution  and  Crossing  Geometry 
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The  flight  statistics  for  the  LQT  solution  for  crossing  geometry  are  shown  in 
Figure  35.  Quadrotor  velocity  reaches  the  simulation  limits  established  by  the  flight 
controller.  The  CPA  occurs  at  the  time  of  intercept,  approximately  10  s.  The  slope  of  the 
position  error  changes  throughout  the  intercept,  reflecting  the  course  and  speed  changes 
made  in  the  flight  path. 
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Figure  35.  Flight  Statistics  of  LQT  Solution  and  Crossing  Geometry 


The  trajectory  plot  for  the  LQT  solution  for  head-on  geometry  is  shown  in  Figure 
36.  The  simulated  quadrotor  shows  some  tracking  error  through  the  turn  immediately 
before  intercept.  The  flight  path  for  this  optimal  solution  is  not  as  direct  as  the  PN  results, 
but  the  end  position  of  the  trajectory  is  in  the  preferred  location  ahead  of  the  target. 
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Figure  36.  Trajectory  Plot  of  LQT  Solution  and  Flead-on  Geometry. 


The  flight  statistics  for  the  LQT  solution  for  head-on  geometry  are  shown  in 
Figure  37.  The  CPA  occurs  at  the  time  of  intercept,  approximately  10  s. 


Figure  37.  Flight  Statistics  of  LQT  Solution  and  Flead-on  Geometry 

The  trajectory  plot  for  the  LQT  solution  for  tail-chase  geometry  is  shown  in  Figure 
38.  The  simulated  quadrotor  shows  some  tracking  error  through  the  turn  immediately 
before  intercept.  The  flight  path  shows  some  maneuvering  but  is  generally  a  direct 
intercept. 
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Figure  38.  Trajectory  Plot  of  LQT  Solution  and  Tail-chase  Geometry 

The  flight  statistics  for  the  LQT  solution  for  tail-chase  geometry  are  shown  in 
Figure  39.  The  CPA  occurs  at  the  time  of  intercept,  approximately  10  s. 
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Figure  39.  Flight  Statistics  of  LQT  Solution  and  Tail-chase  Geometry 

The  trajectory  plots  are  combined  and  displayed  in  Figure  40.  The  axes  and  scale 
are  omitted  but  unchanged  from  their  representation  in  Figure  16  through  Figure  38. 
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Figure  40.  Combined  Results 
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The  time  to  intercept  (seconds)  for  each  planner  and  geometry  type  are  shown  in 
Table  3. 


Table  3.  Experimental  Intercept  Times 


MPC 

PN 

PG 

LQT 

Crossing 

20.3 

17.1 

30.6 

9.9 

Head-on 

13.2 

12.5 

12.8 

9.8 

Tail-chase 

24.6 

19.2 

22.1 

9.7 

The  energy  used  (joules)  by  the  simulated  quadrotor  during  the  experimental 
intercept  for  each  planner  and  geometry  type  are  shown  in  Table  4. 


Table  4.  Experimental  Intercept  Energy 


MPC 

PN 

PG 

LQT 

Crossing 

2931.5 

1440.9 

2546.1 

930.6 

Head-on 

1118.5 

1068.8 

1091.0 

941.5 

Tail-chase 

2928.8 

1608.8 

1848.8 

837.7 

A  summary  of  experimental  intercept  times  and  total  energy  is  shown  in  Figure 
41.  Results  are  grouped  by  geometry  type  and  colored  based  on  trajectory  planner. 
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Figure  41.  Summary  of  Experimental  Data 
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VIII.  CONCLUSION  AND  FUTURE  WORK 


A.  CONCLUSION 

The  research  objective  was  to  determine  if  optimal  control  can  outperform  classic 
missile  control  methods  when  applied  to  quadrotor  drones.  This  is  achieved  by  simulating 
the  intercept  of  a  target  with  a  quadrotor  drone  and  comparing  the  performance  measure 
of  each  trajectory  planner.  The  trajectory  planner  with  the  best  performance  measure  in 
simulation  represents  the  best  planner  for  actual  flight  trajectory  planning. 

A  set  of  optimal  but  off-line  intercept  trajectories  are  calculated  as  a  benchmark. 
Each  of  the  on-line  trajectory  planners  (PG,  PN,  and  MPC)  executes  an  intercept 
maneuver  while  the  time  and  energy  spent  by  a  simulated  quadrotor  are  recorded.  Then, 
the  benchmark  optimal  trajectories  are  flown  by  the  simulated  quadrotor,  and  the  same 
time  and  energy  data  are  recorded.  A  comparison  of  the  trajectories  and  a  summary  of  the 
flight  statistics  highlight  the  strengths  and  weaknesses  of  each  planner. 

The  optimal  results  when  flown  by  the  simulated  quadrotor  indeed  show  the 
highest  intercept  performance.  The  intercept  occurs  in  all  three  geometries  at  just  before 
10  s,  with  total  energy  spent  to  intercept  below  1000  J.  Because  this  solution  requires 
knowledge  of  the  flight  path  of  the  target  over  the  full  simulation  time,  this  method 
cannot  be  used  to  calculate  an  on-line  command  trajectory. 

The  best  on-line  performance  observed  is  the  PN  planner.  It  consistently  achieves 
an  intercept  in  the  least  amount  of  time  and  with  the  least  amount  of  energy  across  all 
geometry  types.  The  low  energy  consumption  is  consistent  with  the  minimal 
maneuvering  for  each  of  the  intercepts. 

The  next  best  performance  after  PN  is  both  the  MPC  and  PG  planners.  Their 
results  are  mixed  based  on  geometry.  For  crossing  geometry,  MPC  is  faster  but  uses  more 
energy  than  PG.  This  is  acceptable  in  a  situation  where  intercept  time  is  more  important 
than  stretching  the  flight  endurance  of  the  aircraft.  For  head-on  geometry,  PG  is  both 
faster  and  uses  less  energy  than  MPC.  The  margin,  however,  is  very  small — less  than  a 
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second  and  a  100  joule  difference.  Finally,  tail-chase  results  show  that  PG  uses  less  time 
and  energy  than  MPC. 

B.  FUTURE  WORK 

The  MPC  steady  state  error  is  the  cause  of  its  low  performance.  Improving  the 
configuration  of  this  planner  in  future  work  may  produce  better  results  by  the  MPC 
trajectory  planner. 

Target  maneuvers  are  likely  to  have  an  impact  on  the  performance  of  the  PN 
trajectory  planner,  as  suggested  by  [4],  [5],  and  [7].  The  effects  of  target  maneuvers  on  all 
guidance  types  should  be  investigated  in  future  work. 

Additionally,  the  estimation  of  energy  consumption  and  intercept  time  by  a 
simulated  quadrotor  should  be  validated  with  physical  implementation.  The  motor, 
electronic  speed  control  (ESC),  and  propeller  used  in  the  simulation  should  be  the  same 
as  those  used  on  any  future  real  hardware. 
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APPENDIX  A.  MOTOR  AND  PROPELLER  THRUST 

MEASUREMENT 


In  order  to  estimate  the  energy  consumption  of  the  simulated  quadrotor,  the  power 
consumption  and  force  of  the  motor  and  propeller  combination  must  be  measured  at  each 
throttle  setting.  An  apparatus  is  used  to  measure  the  force  applied  to  a  sensor  by  the 
motor  and  propeller,  while  another  sensor  measures  current  and  voltage  used  by  the 
motor.  The  throttle  is  stepped  in  five  percent  increments,  holding  each  for  10.0  s  while 
data  is  collected.  The  average  of  the  measurements  at  each  throttle  setting  is  used  to 
produce  a  data  point. 

The  force  and  power  measurement  apparatus  is  shown  in  Figure  42.  The  motor 
and  propeller  are  attached  to  a  lever  arm  balanced  with  a  counterweight.  When  the  motor 
produces  a  force,  it  is  measured  by  the  force  sensor.  The  motor  ESC  is  powered  by  a 
digital  power  supply  which  records  the  voltage,  current,  and  power  at  each  throttle 
setting. 


Counterweight 


Figure  42.  Force  and  Power  Measurement  Apparatus 

The  data  collected  with  the  apparatus  is  displayed  in  Table  5.  The  motor  tested  is 
a  T-Motor  MT2208,  rated  at  1100  kv,  with  a  10x4.5  APC  propeller. 
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Table  5.  Motor  and  Propeller  Performance  Measurements 


Throttle 

Voltage  (V) 

Current  (A) 

Power  (W) 

Thrust  (g) 

0% 

11.99 

0.10 

1.239 

0 

5% 

11.98 

0.10 

1.241 

4 

10% 

11.98 

0.18 

2.136 

17 

15% 

11.98 

0.29 

3.457 

36 

20% 

11.98 

0.43 

5.101 

57 

25% 

11.98 

0.59 

7.03 

77 

30% 

11.98 

0.76 

9.096 

99 

35% 

11.97 

0.95 

11.361 

123 

40% 

11.97 

1.16 

13.935 

146 

45% 

11.97 

1.38 

16.476 

167 

50% 

11.97 

1.57 

18.807 

187 

55% 

11.96 

1.83 

21.868 

211 

60% 

11.96 

2.22 

26.486 

244 

65% 

11.95 

2.68 

31.981 

285 

70% 

11.95 

3.21 

38.334 

329 

75% 

11.94 

3.80 

45.332 

372 

80% 

11.93 

4.46 

53.206 

419 

85% 

11.92 

5.18 

61.771 

466 

90% 

11.92 

6.05 

72.133 

518 

95% 

11.90 

7.03 

83.616 

574 

100% 

11.90 

7.47 

88.876 

599 
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APPENDIX  B.  SIMULINK  AND  MATLAB  CODE 


The  supporting  code  for  the  Simulink  model  and  the  LQT  MATLAB  script  are 
presented  in  this  appendix. 

A.  EXPERIMENTAL  FUNCTION,  EMBEDDED  MATLAB  FUNCTIONS, 
AND  DATA  PLOTS 

The  scripts  and  functions  in  this  section  are  used  in  the  Simulink  model. 

1.  Main  Script 


%  Si  mu  late  a  quadrotor  intercepting  a  moving  target  using  different  methods 
%  of  trajectory  generation.  In  real  time,  generate  intercept  trajectories 
%  and  si  mu  late  a  quadrotor  flying  to  the  co  mma  nd  trajectory.  This  script 
%  runs  the  experimental  simulation  for  each  geo  me  try  type  and  trajectory 
%  generator. 

%At  the  end  of  the  simulation  calculate  and  display: 

%  1.  Plot  of  target  and  command  trajectory  and  path  flown  by  simulated  acft 
%  2 .  Flight  statistics  (velocity,  distance  to  tgt) 

%  3 .  3d  view  of  aircraft  availaible  in  si  mu  link  model 

%  By  Rob  Allen 
% 

cl  c 

clear  all 
close  all 

mpc i  n i  t ; 

open_system(  '  QuadJ  ntercept'  ) 

%  Select  trajectory  planner  for  intercept 
%  1  =  MPC  finite  hor  i  z  i  on 
%  2  =  LQR  infinite  horizion 
%  3  =  Proportional  Navigation 
%  4  =  Pursuit  guidance 

%  i  n  i  t  i  a  I  i  z  e  g  e  o  me  t  r  y 
%  1  =  crossing 
%  2  =  head  on 
%  3  =  tail  chase 

%  Run  experiment  for  each  geo  me  try  and  each  trajectory  planner 
for  geo  me  try  =  1:3 
for  t  r  j  _  s  e  I  =  1:4 
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switch  g  eo met  r  y 
case  1  %c  r  o  s  s  i  n  g 

%  set  target  initial  conditions 

V  =  7; 

heading  =  -  90; 

Vx  =  V*cosd( headi  ng) ; 

V y  =  V*si  nd( headi  ng) ; 
t  _ x 0  =  [  5  0  1  0  0  -  2  Vx  V y  0] ; 

%  set  interceptor  initial  conditions 
i  _ x 0  =  [  -  1  0  0  2  5  0]  ; 

%  set  Interceptor  trajectory  initial  conditions 
i t  _  x  0  =  [ -  1  0  0  2  5  -  2] ; 

%l  o  a  d  optimal  trajectory  (only  used  by  trajectory  4) 
I  oad('  opti  mal  _crossi  ng.  mat') 
case  2  %  head  on 

%  set  target  initial  conditions 

V  =  7; 

heading  =  -165; 

Vx  =  V*cosd( headi  ng) ; 

V y  =  V*si  nd( headi  ng) ; 
t  _ x 0  =  [  1  0  0  2  5  -  2  Vx  V y  0] ; 

%  set  Interceptor  initial  conditions 
i  _ x 0  =  [  -  1  0  0  2  5  0]  ; 

%  set  Interceptor  trajectory  initial  conditions 
i t  _  x  0  =  [ -  1  0  0  2  5  -  2] ; 

%l  o  a  d  opti  mal  trajectory  (only  used  by  trajectory  4) 
I  o  a  d  ( ‘  opti  mal  _  h  e  a  d  o  n .  mat'  ) 
case  3  %  t  a  i  I  chase 

%  set  target  initial  conditions 

V  =  7; 

heading  =  20; 

Vx  =  V*cosd( headi  ng) ; 

V y  =  V*si  nd( headi  ng) ; 
t  _ x 0  =  [  -  5  0  -  2  5  -  2  Vx  V y  0] ; 

%  set  Interceptor  initial  conditions 
i  _ x 0  =  [  -  1  0  0  2  5  0]  ; 

%  set  Interceptor  trajectory  initial  conditions 
i t  _  x  0  =  [ -  1  0  0  2  5  -  2] ; 

%l  o  a  d  opti  mal  trajectory  (only  used  by  trajectory  4) 
I  oad('  opti  mal  _tai  I  chase,  mat'  ) 
ot  he r  wi  s  e 
end 


58 


[  tcpa(geometry,  t  r  j  _  s  e  I  )  ,  cpa(  geomet  ry,  t  r  j  _  s  e  I  )  ,  t  energy(  geomet  ry,  t  r  j  _  s  e  I  )  ]  = 
experi  ment(trj_sel  ,  geo  me  try); 

end 

end 

%  P I  o  t  final  e  x  p  e  r  i  me  n  t  statistics 

pi  ot_experi  mental  _  r  e  s  u  I  ts; 

filename  =  '  output\testdata.  xl  sx’  ; 
xl  swri  t  e  ( f  i  I  ename,  tenergy,  1,  '  B2:  E4 '  ) 
xl  swri  t  e  ( f  i  I  ename,  tcpa,  1,  '  B8:  E 1 0  ’  ) 
save(  '  output\resul  ts'  )  ; 


2.  Experiment  Function 


function  [  tcpa,  cpa,  tenergy  ]  =  experiment)  planner,  geometry) 

%  experi  me  nt(  planner,  geometry)  runs  one  quadrotor  simulation  with  the  given 
%  planner  and  geometry  type.  Execute  the  Quad_i  ntercept,  si  x  simulation  and 
%  return  time  to  CPA,  CPA  and  Total  Energy. 

%  Trajectory  Planners  are  defined  in  the  si  mu  link  mo  del  by: 

%  1  =  MPC  finite  hor  i  z  i  on 
%  2  =  LQR  infinite  horizion 
%  3  =  Proportional  Navigation 
%  4  =  Pursuit  guidance 

%  Si  mu  late  a  quadrotor  intercepting  a  moving  target  using  different  methods 
%  of  trajectory  generation.  In  real  time,  generate  intercept  trajectories 
%  and  si  mu  late  a  quadrotor  flying  to  the  co  mma  nd  trajectory. 

%  At  the  end  of  the  simulation  calculate  and  display: 

%  1.  Plot  of  target  and  command  trajectory  and  path  flown  by  simulated  acft 
%2.  Flight  statistics  (velocity,  distance  to  tgt,  power  and  energy 
%  3 .  3d  view  of  aircraft  availaible  in  si  mu  link  model 

%  By  Rob  Allen 
% 

t  r  j  _  s  e  I  =  planner; 
si  m(  '  Quad_i  ntercept'  ) 
axislimits  =  120; 

v  =  [  st  at  e(  :  ,  4)  state(  :  ,  5 )  st  at  e(  :  ,  6)  ] ; 
pos  =  [state(;, 1)  state!:, 2)  state!:, 3)]; 

%To  display  simulation  in  the  NED  fra  me,  for  mat  p  I  o  1 3  ( y ,  x,  -z) 
f  i  gure(  1) 

plot3(-pos(:,2),-pos(:,l),-pos(:,3),  1  Li  n  e  Wi  d  t  h '  ,  2) 
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hoi  d  on 

pi  ot3(  -  pos_t(  :  ,  2)  ,  -  pos_t(  :  ,  1)  ,  -  pos_t(  :  ,  3)  ,  'LineWidth',  2 ) 

pi  ot3(-trj  (  :  ,  2),  -  trj  (:  ,  1),  -  trj  (  :  ,  3),  'LineWidth',  1) 

hold  off 
grid  on 

a  x  i  s  ( [  -  a  x  i  s  I  i  mi  t  s ,  a  x  i  s  I  i  mi  t  s ,  -  a  x  i  s  I  i  mi  t  s ,  a  x  i  s  I  i  mi  t  s ,  -  a  x  i  s  I  i  mi  t  s ,  a  x  i  s  I  i  mi  t  s  ]  ) 

xti  ckl  abel  s(  {'  100'  ,  '  5  0 '  ,  '  0 '  ,  '  -  5  0 '  ,  '  -  100'  }) 

yti  ckl  abel  s  (  {'  100'  ,  '  50'  ,  '  O'  ,  '  -  50'  ,  '  -  100'  }) 

x  I  a  b  e  I  ( '  Y  (  m) '  ) 

y  I  a  b  e  I  ( '  X  (  m) '  ) 

zl  abel  ( '  V  ) 

v i  e w(  1  8  0,  9  0) 

saveasf  gcf,  [ '  output/G'  ,  num2str(  geometry) ,  '  P'  ,  num2str(  pi  anner)  ,  '  .  eps'  ]  ,  '  epsc'  ) 
v  =  [state(:, 4)  state!:, 5)  state!:, 6)]; 

%  Plot  and  save  flight  statistics  and  return  flight  me  tries 
%  p o s _ t  :  position  of  target 
%  v  :  velocity  of  target 

%  pos_er  ror  :  position  of  target  minus  position  of  interceptor 
%  Ft  :  Thrust  force  applied  to  interceptor 
%  t  :  t  i  me  vector 
%  n  :  e  x  p  e  r  i  me  n  t  number 

%  v  t  r  j  :  velocity  of  interceptor  trajectory 

simname  =  'Flight  Statistics';  %c h a n g e  to  PID  guidance  later 

v  e  I  _  t  =  v  t  r  j  ; 

vj  =  sqrt  (  v(  :  ,  1) .  A2  +  v(  : ,  2)  .  A2  +  v(  :  ,  3)  .  A2) ; 

v_t  =  s q r t  (  v e I  _ t  (  :  ,  1 )  .  A2  +  vel  _t  (  :  ,  2)  .  A2  +  vel  _t  (  :  ,  3) .  A2)  ; 

p o s _  e r  r  =  s q r t  (  pos _ e r r o r  (  :  ,  1 )  .  A2  +  pos _ e r r o r  (  :  ,  2 )  .  A2  +  p o s _ e r r or  (  :  ,  3)  .  A2 ) ; 

[CPA,  I  ]  =  mi  n(  p  o  s  _  e  r  r )  ; 

%  P I  o  t  flight  statistics 
f i  gure( 2) 

subpl  ot(2,  1,  1)  %  Velocity  of  interceptor  and  target 
p  I  ot ( t ,  v  _  1 ) 
hoi  d  on 

pi  ot(t(  1:  I  ength(  v_t)  )  ,  v  _  t ) 
hold  off 
grid  on 

yl  abel  (  '  Vel  oci  ty  (m/s)') 
x  I  a  b  e  I  ( '  T  i  me  ( s ) ’  ) 

subpl  ot(2,  1,2)  %  Position  error  (target  minus  trajectory) 
pi  ot  ( t ,  p o s _  e r  r ) 
hoi  d  on 

pi  ot  ( t  (  I  )  ,  CPA,  '  * k '  ) 
hold  off 
grid  on 
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yl  abel  ('  Pos.  Error  (  m)  '  ) 
x  I  a  b  e  I  ( '  T  i  me  ( s ) ’  ) 

saveasf  gcf,  [ '  output/G'  ,  num2str(geometry),  '  P'  ,  nu  m2  str(  planner),  '  stats,  eps 

t  c  pa  =  t ( I  )  ; 
c  pa  =  CPA; 

tenergy  =  energy(end); 
end 


3.  Initialize  MPC  Script 


%  Calculate  an  intercept  trajectory  of  a  moving  target  for  a  quadrotor 
%  using  si  mu  link  MPC  block.  This  file  creates  the  MPC  object  necessary  for 
%  t  h  e  s  i  mu  I  i  n  k  block. 

%  By  Rob  Allen 
% 


%d e f  i  n e  a  system  x d o t  =  Ax  +  Bu,  y  =  Cx  +  Du 


A 

= 

[0 

0 

0 

1 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

B 

= 

[0 

0 

0; 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

1 

0 

0 

0 

l); 

C 

= 

[  1 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

1 

D 

= 

[0 

0 

0; 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

01; 

states 

= 

{' 

X  ' 

’  ] . ’  eps c ' ) 
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inputs  =  { '  f  x '  '  f  y '  '  f  z '  } ; 

outputs  =  { '  x '  '  y '  1  z '  '  v  x '  '  v  y '  '  v  z '  } ; 

%c  r  e  a  t  e  open  loop  system 

s  y  s  _  s  s  =  ss(A,  B,  C,  D,  '  statename'  ,  states,  '  i  nputname'  ,  i  nputs,  '  outputname'  ,  outputs) ; 

%c  heck  controllability 
co  =  ctrb(sys_ss); 

%  Setup  the  MFC  block  and  limit  input 
T  s  =  0.1;  %  T  i  me  step 
p  =  5  0;  %  Prediction  Horizion 

m  =  2  0 ;  %  Control  Horizion 

umax  =4;  %  Max  input 

mpc  obj  =  mpc(sys_ss,  Ts ,  p,  m)  ; 

mpcobj.MV  =  structf  '  Mi  n'  ,  {-  umax;  -  umax;  -  umax},  '  Max’  ,  {umax;  umax;  umax},  ... 

'  RateMi  n'  ,  {  -  1;  -  1;  -  1})  ;  %S  p  e  c  i  f  y  input  saturation  limits 
mpcobj  .  Wei  ght  s  =  s  t  r  u  c  t  ( 1  MV' ,  [  1  1  1  ]  ,  '  MV  Rate'  ,  [  1  1  l],'OV',  ... 

[1  1  1  1  1  1]);  %  define  weights  on  manipulated  and  controlled  variables 

%  Define  limits  on  velocity  by  creating  a  state  constraint 


v  ma  x  =  9 


%  mpcobj 

.  OV( 4)  . 

Mi  n  = 

-  v  ma  x ; 

%  mpcobj 

.  OV(  4)  . 

Max  =  vmax; 

%  mpcobj 

.  OV(  5)  . 

Mi  n  = 

-  v  ma  x ; 

%  mpcobj 

.  OV(  5)  . 

Max  =  vmax; 

%  mpcobj 

.  OV(  6)  . 

Mi  n  = 

-  v  ma  x ; 

%  mpcobj 

.  O V(  6)  . 

Max  =  vmax; 

%  Set  constraints  in  the  form  E*u(  k+j  )  +F*y(  k+j  )  <=G 
E  =[  0  0  0; 

0  0  0; 

0  0  0; 

0  0  0; 

0  0  0; 

0  0  0]; 

F  =  [  0  0  0  0  0  0; 

000000; 

000000; 

000100; 

000010; 

000001]; 

G  =  [  0 ;  0;  0;  vmax;  vmax;  v  ma  x  ]  ; 

%  select  Equal  Concern  for  Relaxation  of  constraints 
V  =  [  1;  1;  1;  0.  001;  0.  001;  0.  001]  ; 

setconstrai  nt(  mpcobj  ,  E,  F,  G,  V)  ; 
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4, 


Embedded  Function  “idyn” 


function  [sys,  xO,  str,  ts]  =  idyn(t,x,u,fl  ag, 
%  Interceptor  dynamics  for  Quad_i  ntercept.  si  x 
%  By  Rob  Allen 


if  a  bs  ( f  I  a  g )  ==  1 

w  =  u(  1)  ; 

A  =  [  0  0  0  1  0  0; 
000010; 
000001; 

0  0  0  0  -  w  0 ; 

0  0  0  w  0  0 ; 
000000]; 

sys(  1:  6)  =  A* x ( 1:  6) 


elseif  flag  ==  3 
sys(  1:  6)  =  x(  1:  6)  ; 

elseif  flag  ==  4 
samp  I  e  T  i  me  =  0.1; 
sys  =  t  +  samp  I  e  T  i  me; 

elseif  flag  ==  0 
sizes  =  s  i  ms  i  z  e  s ; 
sizes.NumContStates  =  6; 
si  zes.  NumDi  scStates  =  0 ; 
sizes. NumOutputs  =  6; 
s  i  z  es .  Nu  ml  n  p u t  s  =  1 ; 
si  zes.  Di  rFeedthrough=  1; 
si  zes.  NumSampI  eTi  mes=  1 ; 
sys  =  s  i  ms  i  z  e  s  ( s  i  z  e  s )  ; 

%  i  n  i  t  i  a  I  conditions 
V  =  9; 

heading  =  0; 

Vx  =  V*cosd(  headi  ng)  ; 

V y  =  V*si  nd( headi  ng)  ; 

x  0  =  [  f  t  _  x  0  (  1)  i  t  _  x  0  (  2)  i  t  _  x  0  (  3)  Vx  V  y  0]  ; 
str  =  [  ]  ; 
ts  =  [  0  0  ] ; 
else 

sys  =  [  1  ; 
end 


i  t  _  x  0 ) 
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5, 


Embedded  Function  “tdyn” 


function  [sys,  xO,  str,  ts]  =  t  d  y  n  ( t ,  x ,  u ,  f  I 
%  Target  dynamics  for  Quad_i  ntercept.  si  x 
%  By  Rob  Allen 
% 


if  a  bs  ( f  I  a  g )  ==  1 

w  =  u(  1) ; 
w_  =  0 ; 

R1  =  [  0  -  1  0; 

1  0  0; 

0  0  1]; 

R2  =  [  0  0  -  1; 

0  1  0; 

10  0]; 

R3  =  [  1  0  0; 

0  1  0; 

0  0  1]; 

R  =  w_  *  R  2  +  (  w*  R 1 )  ; 

A  =  [  0  0  0  1  0  0; 

000010; 

000001; 

0  0  0  R(  1,  1)  R(  1,  2)  R(  1,  3) ; 

0  0  0  R(  2,  1)  R(  2,  2)  R(  2,  3) ; 

0  0  0  R(  3,  1)  R(  3,  2)  R(  3,  3) ] ; 

sys( 1:  6)  =  A* x ( 1:  6)  ; 


elseif  flag  ==  3 
sys( 1:  6)  =  x( 1:  6) ; 

elseif  flag  ==  4 
samp  I  e  T  i  me  =  0.01; 
sys  =  t  +  samp  I  e  T  i  me; 

elseif  flag  ==  0 
sizes  =  s  i  ms  i  z  e  s ; 
sizes.NumContStates  =  6; 
si  zes.  NumDi  scStates  =  0 ; 
sizes.NumOutputs  =  6; 
sizes.  Nu  ml  n p u t  s  =  2; 
si  zes.  Di  rFeedthrough=  1; 
si  zes.  NumSampi  eTi  mes=  1 ; 
sys  =  s  i  ms  i  z  e  s  ( s  i  z  e  s )  ; 


a  g ,  t  _  x  0 ) 
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%  i  n  i  t  i  a  I  conditions 
xO  =  t  _ x  0 ; 
str  =  [  1  ; 
ts  =  [  0  0  ] ; 
else 

sys  =  [ ]; 
end 


6.  Embedded  Function  “state_eq” 


function  [sys,  xO,  str,  ts]  =  st  at  e  _  e  q  ( t ,  x,  u,  f  I  ag,  i  _  x  0 ) 
%  Quadrotor  dynamics  for  Quad_i  ntercept.  si  x 
%  By  Rob  Allen 

% 

g  =9.8; 
m  =  0.812; 


J  x  =  0.0  0  6; 

J  y  =  0.0  0  6; 
j  z  =0.012; 

J  =  [J  x  0  0; 

0  j  y  0; 

0  0  j  z]  ; 

if  a  bs  ( f  I  a  g )  ==  1 

ft  =  u(  1)  ; 
t  au  =  u(  2:  4)  ; 
phi  =  x  (  7 )  ; 
theta  =  x ( 8 ) ; 
psi  =  x(  9) ; 

w(  1:  3)  =  x(  10:  12)  ; 
w  =  w'  ; 

Rpsi  =  [  cos(  psi  )  -  si  n(  psi  )  0; 
si  n  (  psi  )  c  o  s  (  psi  )  0; 

0  0  1] ; 

Rtheta  =  [  cos(theta)  0  sin(theta); 
0  1  0  ; 

-si  n(theta)  0  c  os  ( t  het  a )  ] ; 

Rphi  =  [  1  0  0; 

0  c  os  (  phi  )  -  s  i  n  (  ph  i  )  ; 

0  si  n  (  phi  )  c  os  (  ph  i  )  ] ; 

Rib  =  Rp  s  i  *  Rt  h  et  a  *  Rp  h  i  ; 
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Q  =  [  1  0  -si  n(theta); 

0  cos(phi)  sin(phi)*cos(theta); 

0  -si  n (  p h i  )  c os  (  phi  )  * c os  ( t  het  a )  ] ; 

M  =  [  0  -  w(  3 )  w(  2) ; 

w(  3)  0  -  w(  1)  ; 

-  w(  2 )  w(l)  0  ]; 

sys( 1:  3)  =  x( 4:  6) ; 

s  y  s  (  4 :  6 )  =  [  0 ;  0;  g  ]  -  Rib/m*[0;  0;  ft]; 

s  y  s  (  7:  9)  =  i  n  v  (  Q)  *  w; 

sys  (  10:  12)  =  J  A-  1*(  t  au-  M*J  *w) ; 

el  s e i  f  flag  ==  3 

sys{ 1;  12)  =  x( 1:  12)  ; 

elseif  flag  ==  4 
samp  I  e  T  i  me  =  0.01; 
sys  =  t  +  samp  I  e  T  i  me; 

elseif  flag  ==  0 
sizes  =  s  i  ms  i  z  e  s ; 
si  zes.  NumContStates  =  12; 
si  zes.  NumDi  scStates  =  0 ; 
sizes. NumOutputs  =  12; 
sizes.  Nu  ml  n  p u t  s  =  4; 
si  zes.  Di  rFeedthrough=  1; 
si  zes.  NumSampi  eTi  mes=  1 ; 
sys  =  s  i  ms  i  z  e  s  ( s  i  z  e  s )  ; 

%  initial  conditions 

xO  =  [  i  _  x  0  (  1 )  i  _  x  0  (  2)  i  _  x  0  (  3  )  0  0  0  0  0  0  0  0  0  ]  ; 
str  =  [  ]  ; 
ts  =  [  0  0  ]  ; 
else 

sys  =  [ 1 ; 
end 


B.  OFF-LINE  RESULTS,  LQT  METHOD 

The  script  in  this  section  is  used  to  create  off-line  LQT  trajectories. 

%  Plot  optimal  intercept  trajectory  using  LQT  and  straight  line  motion 
%  t  a  r  g  e  t  trajectory. 

%  By  Rob  Allen 

% 

cl  c 

clear  all 
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close  all 


%  system  p  a  r  a  me  t  e  r  s 
A  =  [  0  0  1  0; 

0  0  0  1; 

0  0  0  0; 

0000]; 

B  =  [  0  0; 

0  0; 

1  0; 

0  1]; 

H  =  [  1  0  0  0  0  0  0  0  0; 

0  100000  0  0; 

0  0  10; 

0001]; 

Q  =  [  1  0  0  0; 

0  10  0; 

0  0  10; 

0001]; 

R  =  100*]  1  0; 

0  1]; 

tf  =  35; 
dt  =0.1; 


x 0  =  [25;  -  100;  0;  0]  ; 
x _ t  f  =[0;  0;  0;  0]  ; 

%  Ref  rence  trajectory 
%  1:  x  =5  0  y  =  1 0  0  v=7  t  het  a=-  90  (Crossing) 

%  2:  x  =  1 0  0  y  =2  5  v=7  t  het  a=-  165  (Head  on) 

%  3:  x  =-  5  0  y  =-  2  5  v=7  theta  =2  0  (Tall  chase) 
rv  =  7; 

%  c  r  o  s  s  i  n  g 
theta  =  -  90; 

rO  =  [100;  50;  rv*si  nd(theta);  rv*cosd(  t  het  a)  ] ; 

%  Head  on 
%  theta  =  - 165; 

%  rO  =  [25;  100;  rv’sind(theta);  rv*cosd(theta)); 


%  Tail  chase 
%  t  h  e  t  a  =  2  0; 

%  rO  =  [-25;  -50;  rv*si  nd(theta);  rv*cosd(t  heta)  ] ; 
I  =0; 

for  t  =0:  dt :  t  f 
I  =1+1; 
t  i  me (  I  )  =  t ; 
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r_t ( 1:  2,  I  )  =  r 0(  3:  4)  *t  +  r 0( 1:  2) ; 
r_t ( 3:  4,  I  )  =  [  r  0 ( 3) ;  r  0 (  4)  ] ; 
end 


t  mp  1  =  -  B*inv(R)’B'; 
A.t  =  -  A'  ; 

A  bar  =  ... 


[  A(  1,  1)  A(  1,  2)  A(  1,  3)  A(  1,  4)  t  mp 1 (  1,  1) 
A(  2,  1)  A(  2,  2)  A(  2,  3)  A(  2,  4)  t  mp  1  (  2 ,  1 ) 

A(  3,  1)  A(  3,  2)  A(  3,  3)  A(  3,  4)  t mp 1 (  3,  1) 

A(  4,  1)  A(  4,  2)  A(  4,  3)  A(  4,  4)  t  mp  1  (  4,  1) 

-Q(  1,  1)  -  Q(  1,  2)  -Q(  1,3)  -Q(  1,  4)  A _ t  (  1 , 

-  Q(  2,  1)  -  Q(  2,  2)  -  Q(  2,  3)  -  Q(  2,  4)  A.t  (  2, 

-  Q(  3,  1 )  -  Q(  3,  2)  -  Q(  3,  3)  -  Q(  3  ,  4)  A.t(3, 

■  Q(  4,  1)  -  Q(  4,  2)  -  Q(  4,  3)  -  Q(  4,  4)  A.t  (4, 


t  mp  1  (  1 ,  2 )  t  mp  1  (  1 ,  3 ) 
t  mp  1  (  2 ,  2 )  t  mp 1 (  2 ,  3 ) 
t  mp  1  (  3 ,  2 )  t  mp 1 (  3 ,  3 ) 
t  mp  1  (  4 ,  2 )  t  mp 1 (  4 ,  3 ) 
1)  A.t  (  1,  2)  A.t  (  1,  3) 
1)  A.t ( 2 ,  2 )  A.t ( 2 ,  3 ) 
1)  A.t  (3,2)  A.t  (3,3) 
1)  A.t  (  4,  2 )  A.t  (  4,  3 ) 


t  mp 1  (  1 ,  4 ) 
t  mp 1  (  2 ,  4 ) ; 
t  mp 1  {  3,  4) ; 
t  mp 1  (  4 ,  4 ) ; 
A.t  (  1,  4) ; 
A.t  (  2,  4) ; 
A.t  (  3,  4) ; 
A.t  (  4,  4) ] 


eA.bartf  =ex  p  m(  A.  ba  r  *  t  f )  ; 


B.bar  =  dt*[  0  0 

0  0  0 

0  0  0 

0  0  0 

0(1,  1)  Q(  1,  2) 
Q(  2,  1)  Q(  2,  2) 
0(3,1)  Q(3,2) 
0(4,1)  Q(4,2) 


0  0; 

0; 

0; 

0; 

Q(  1,  3)  0(1,  4)  ; 
0(2,3)  0(2,4); 
0(3,3)  Q(3,4); 
0(4,3)  Q(4,4)]; 


%  Solve  for  p  ( 0 )  and  x  ( t  f ) 
t  mp 1  =  0 ; 

I  =0; 

for  tau  =  0 :  d t : t  f 
I  =1  +1; 

tmpl  =  tmpl  +  expm(A_bar*(tf-tau)  )*B.bar*r.t(  : ,  I ); 
end 


el  =  eA.bartf(  1:  8,  1:  4)  ; 
e2  =  eA.bart  f  (  1:  8,  5:  8)  ; 

A(  1:  4, 1:  4)  =  eye( 4)  ; 

A(  5:  8,  1:  4)  =  H; 

A(  1:  8,  5:  8)  =  -  e2; 

B1  =  e  1  *  x  0  +  tmpl  +  [zeros(4,  4)  ;  H]*r.t(  :  ,  end)  ; 

X  =  i  n  v ( A)  *  B 1 ; 
xtf  =  X( 1:  4)  ; 
pO  =  X(  5:  8)  ; 

%  solve  for  the  state  and  co-state 
I  =0; 
j  =  0; 

wO  =  [ xO;  pO]  ; 
for  t  =0:  dt :  t  f 
I  =1+1; 
t  i  me (  I  )  =  t ; 
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%  I  n  t  e  g  r  a  I  term 
t  mp 1  =  0 ; 

II  =0; 

for  t  a  u  =  0:  dt :  t 
II  =11  +1; 

t  mp  1  =  t  mp  1  +  expm(A_bar*(t-tau))*B_bar*r_t( : ,  I  I  ); 
end 

w_  t  ( :  ,  I  )  =  expm(  A_bar*t )  *w0  +  tmpl; 
x  =  [  w_  t  (  1 ,  I  ) ;  w_t(2,l);  w_  t  (  3 ,  I  ) ;  w_  t  (  4 ,  I  )  ]  ; 

p  =  [  w_  t  (  5 ,  I ) ;  w_  t  (  6 ,  I  ) ;  w_  t  (  7 ,  I  ) ;  w_  t  {  8 ,  I )  ]  ; 

r  =  r_t  (  1:  4,  I  ) ; 
u  (  :  ,  I  )  =-  i  n v (  R )  *  B '  *  p; 

j  =  j  +  (  (  x  -  r)  1  *  Q  *  (  x  -  r)  +  u( : , I  )  1  *  R  *  u(  : , I  )  )  *  dt 

end 


%  compute  the  optimal  cost  and  velocity 
j  =  0.  5*( x-  r )  '  *  H*  (  x- r )  +  0 .  5  *  J 
v  =  sqr t (  w_t  (  3,  :  )  .  "2  +  w_  t (  4 ,  :  )  .  ^  2 )  ; 


%  pi  ot 

axislimits  =  120; 
figure 

pi  ot  ( r_t  ( 1,  :  ) ,  r_t  ( 2,  :  ) ,  'color', 
hoi  d  on 

pi  ot  (  w_t  (  1,  :  )  ,  w_t  ( 2,  :  ) ,  'color', 
hold  off 
x  I  a  b  e  I  ( '  Y  (  m)  ’  ) 
y  I  a  b  e  I  (  '  X  (  m)  '  ) 
grid  on 


[0.8  5  0  0  0.3  2  5  0  0.0  9  8  0],  '  L  i  n  e  Wi  d  t  h '  ,  2 ) 

[0.9  2  9  0  0.6  9  4  0  0.1  2  5  0],  '  L I  n  e  Wi  d  t  h '  ,  2 ) 


a  x  i  s  ( [  -  a  x  i  s  I  i  mi  t  s ,  a  x  i  s  I  i  mi  t  s ,  -  a  x  i  s  I  i  mi  t  s ,  a  x  i  s  I  i  mi  t  s  ]  ) 
s  a  v  e  a  s  ( gcf,  [  ’  output  /  H '  ,  num2str(theta)  ,  '  _  t  r  a  j  ectory.  e  p  s  ’  ]  ,  1  e  p  s  c '  ) 


figure 

subpl  o t  ( 2 ,  1,  1) 

p I  ot ( t  i  me ,  u ) 

x  I  a  b  e  I  ( '  1 1  me ( s ) 1 ) 

yl  abel  ( '  cont  rol  input') 

ti  tl  e(  [  'Total  cost:  ',  n  u  m2  s  t  r  ( J  )  ] ) 

I  egend( '  F_x' , ' F_y' ) 


subpl  o  t  (  2 ,  1,  2) 
pi  o t  ( t  i  me ,  v ) 
x  I  a  b  e  I  ( '  1 1  me ( s ) 1 ) 
ylabel('veloclty') 
title('Velocity') 

saveasf  gcf,  [ '  output  /  H’  ,  nu  m2  str(theta),  '_stats.eps'],  '  e  p  s  c '  ) 
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